/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*  Contributing Authors (specific to this file):
*  Anthony Rowe
*******************************************************************************/



#include <include.h>
#include <ulib.h>
#include <stdlib.h>
#include <avr/sleep.h>
#include <avr/wdt.h>
#include <avr/eeprom.h>
#include <stdio.h>
#include <avr/interrupt.h>
#include <nrk.h>
#include <nrk_events.h>
#include <nrk_timer.h>
#include <nrk_error.h>
#include <nrk_reserve.h>
#include <bmac.h>
#include <nrk_cfg.h>


#ifndef BMAC_STACKSIZE
#define BMAC_STACKSIZE 128
#endif

static nrk_task_type bmac_task;
static NRK_STK bmac_task_stack[BMAC_STACKSIZE];


//#define DEBUG
static uint32_t rx_failure_cnt;

static uint8_t tx_data_ready;
static uint8_t rx_buf_empty;
static uint8_t bmac_running;
static uint8_t pkt_got_ack;
static uint8_t g_chan;
static uint8_t is_enabled;

static nrk_time_t _bmac_check_period;

static uint8_t cca_active;
static int8_t tx_reserve;

static nrk_time_t dummy_t;

/**
 *  This is a callback if you require immediate response to a packet
 */
RF_RX_INFO *rf_rx_callback (RF_RX_INFO * pRRI)
{
  // Any code here gets called the instant a packet is received from the interrupt   
  return pRRI;
}

int8_t bmac_encryption_set_ctr_counter (uint8_t * counter, uint8_t len)
{
  if (len != 4)
    return NRK_ERROR;
  rf_security_set_ctr_counter (counter);
  return NRK_OK;
}

int8_t bmac_tx_reserve_set (nrk_time_t * period, uint16_t pkts)
{

#ifdef NRK_MAX_RESERVES
// Create a reserve if it doesn't exist
  if (tx_reserve == -1)
    tx_reserve = nrk_reserve_create ();
  if (tx_reserve >= 0)
    return nrk_reserve_set (tx_reserve, period, pkts, NULL);
  else
    return NRK_ERROR;
#else
  return NRK_ERROR;
#endif
}

uint16_t bmac_tx_reserve_get ()
{
#ifdef NRK_MAX_RESERVES
  if (tx_reserve >= 0)
    return nrk_reserve_get (tx_reserve);
  else
    return 0;
#else
  return 0;
#endif
}


int8_t bmac_auto_ack_disable ()
{
  rf_auto_ack_disable ();
  return NRK_OK;
}

int8_t bmac_auto_ack_enable ()
{
  rf_auto_ack_enable ();
  return NRK_OK;
}

int8_t bmac_addr_decode_disable ()
{
  rf_addr_decode_disable ();
  return NRK_OK;
}

int8_t bmac_addr_decode_enable ()
{
  rf_addr_decode_enable ();
  return NRK_OK;
}

int8_t bmac_addr_decode_set_my_mac (uint16_t my_mac)
{
  rf_addr_decode_set_my_mac (my_mac);
  return NRK_OK;
}

int8_t bmac_addr_decode_dest_mac (uint16_t dest)
{
  bmac_rfTxInfo.destAddr = dest;
  return NRK_OK;
}

int8_t bmac_rx_pkt_is_encrypted ()
{
  return rf_security_last_pkt_status ();
}

int8_t bmac_encryption_set_key (uint8_t * key, uint8_t len)
{
  if (len != 16)
    return NRK_ERROR;
  rf_security_set_key (key);
  return NRK_OK;
}

int8_t bmac_encryption_enable ()
{
  //rf_security_enable();
  return NRK_OK;
}

int8_t bmac_encryption_disable ()
{
  //rf_security_disable();
  return NRK_OK;
}


int8_t bmac_set_rf_power (uint8_t power)
{
  if (power > 31)
    return NRK_ERROR;
  rf_tx_power (power);
  return NRK_OK;
}

void bmac_set_cca_active (uint8_t active)
{
  cca_active = active;
}

int8_t bmac_set_cca_thresh (int8_t thresh)
{
  rf_set_cca_thresh (thresh);
  return NRK_OK;
}

int8_t bmac_set_channel (uint8_t chan)
{
  if (chan > 26)
    return NRK_ERROR;
  g_chan = chan;
  rf_power_up ();
  rf_init (&bmac_rfRxInfo, chan, 0xFFFF, 0x00000);
  return NRK_OK;
}


int8_t bmac_wait_until_rx_pkt ()
{
  nrk_sig_mask_t event;

  if (bmac_rx_pkt_ready () == 1)
    return NRK_OK;

  nrk_signal_register (bmac_rx_pkt_signal);
  event = nrk_event_wait (SIG (bmac_rx_pkt_signal));

// Check if it was a time out instead of packet RX signal
  if ((event & SIG (bmac_rx_pkt_signal)) == 0)
    return NRK_ERROR;
  else
    return NRK_OK;
}

int8_t bmac_rx_pkt_set_buffer (uint8_t * buf, uint8_t size)
{
  if (buf == NULL)
    return NRK_ERROR;
  bmac_rfRxInfo.pPayload = buf;
  bmac_rfRxInfo.max_length = size;
  rx_buf_empty = 1;
  return NRK_OK;
}

int8_t bmac_init (uint8_t chan)
{
  bmac_running = 0;
  tx_reserve = -1;
  cca_active = true;
  rx_failure_cnt = 0;
#ifdef NRK_SW_WDT
#ifdef BMAC_SW_WDT_ID

  _bmac_check_period.secs = 30;
  _bmac_check_period.nano_secs = 0;
  nrk_sw_wdt_init (BMAC_SW_WDT_ID, &_bmac_check_period, NULL);
  nrk_sw_wdt_start (BMAC_SW_WDT_ID);
#endif
#endif



  _bmac_check_period.secs = 0;
  _bmac_check_period.nano_secs = BMAC_DEFAULT_CHECK_RATE_MS * NANOS_PER_MS;
  bmac_rx_pkt_signal = nrk_signal_create ();
  if (bmac_rx_pkt_signal == NRK_ERROR) {
    nrk_kprintf (PSTR ("BMAC ERROR: creating rx signal failed\r\n"));
    nrk_kernel_error_add (NRK_SIGNAL_CREATE_ERROR, nrk_cur_task_TCB->task_ID);
    return NRK_ERROR;
  }
  bmac_tx_pkt_done_signal = nrk_signal_create ();
  if (bmac_tx_pkt_done_signal == NRK_ERROR) {
    nrk_kprintf (PSTR ("BMAC ERROR: creating tx signal failed\r\n"));
    nrk_kernel_error_add (NRK_SIGNAL_CREATE_ERROR, nrk_cur_task_TCB->task_ID);
    return NRK_ERROR;
  }
  bmac_enable_signal = nrk_signal_create ();
  if (bmac_enable_signal == NRK_ERROR) {
    nrk_kprintf (PSTR ("BMAC ERROR: creating enable signal failed\r\n"));
    nrk_kernel_error_add (NRK_SIGNAL_CREATE_ERROR, nrk_cur_task_TCB->task_ID);
    return NRK_ERROR;
  }


  tx_data_ready = 0;
  // Set the one main rx buffer
  rx_buf_empty = 0;
  bmac_rfRxInfo.pPayload = NULL;
  bmac_rfRxInfo.max_length = 0;

  // Setup the cc2420 chip
  rf_power_up ();
  rf_init (&bmac_rfRxInfo, chan, 0xffff, 0);
  g_chan = chan;

//    FASTSPI_SETREG(CC2420_RSSI, 0xE580); // CCA THR=-25
//    FASTSPI_SETREG(CC2420_TXCTRL, 0x80FF); // TX TURNAROUND = 128 us
//    FASTSPI_SETREG(CC2420_RXCTRL1, 0x0A56); 
  // default cca thresh of -45
  //rf_set_cca_thresh(-45); 
  rf_set_cca_thresh (0x0);
  bmac_running = 1;
  is_enabled = 1;
  return NRK_OK;
}

int8_t bmac_tx_pkt_nonblocking (uint8_t * buf, uint8_t len)
{
  if (tx_data_ready == 1)
    return NRK_ERROR;
  tx_data_ready = 1;
  bmac_rfTxInfo.pPayload = buf;
  bmac_rfTxInfo.length = len;
  return NRK_OK;
}

nrk_sig_t bmac_get_rx_pkt_signal ()
{
  nrk_signal_register (bmac_rx_pkt_signal);
  return (bmac_rx_pkt_signal);
}

nrk_sig_t bmac_get_tx_done_signal ()
{
  nrk_signal_register (bmac_tx_pkt_done_signal);
  return (bmac_tx_pkt_done_signal);
}


int8_t bmac_tx_pkt (uint8_t * buf, uint8_t len)
{
  uint32_t mask;
  if (tx_data_ready == 1)
    return NRK_ERROR;
// If reserve exists check it
#ifdef NRK_MAX_RESERVES
  if (tx_reserve != -1) {
    if (nrk_reserve_consume (tx_reserve) == NRK_ERROR) {
      return NRK_ERROR;
    }
  }
#endif
  nrk_signal_register (bmac_tx_pkt_done_signal);
  tx_data_ready = 1;
  bmac_rfTxInfo.pPayload = buf;
  bmac_rfTxInfo.length = len;
#ifdef DEBUG
  nrk_kprintf (PSTR ("Waiting for tx done signal\r\n"));
#endif
  mask = nrk_event_wait (SIG (bmac_tx_pkt_done_signal));
  if (mask == 0)
    nrk_kprintf (PSTR ("BMAC TX: Error calling event wait\r\n"));
  if ((mask & SIG (bmac_tx_pkt_done_signal)) == 0)
    nrk_kprintf (PSTR ("BMAC TX: Woke up on wrong signal\r\n"));
  if (pkt_got_ack)
    return NRK_OK;
  return NRK_ERROR;
}


uint8_t *bmac_rx_pkt_get (uint8_t * len, int8_t * rssi)
{

  if (bmac_rx_pkt_ready () == 0) {
    *len = 0;
    *rssi = 0;
    return NULL;
  }
  *len = bmac_rfRxInfo.length;
  *rssi = bmac_rfRxInfo.rssi;
  return bmac_rfRxInfo.pPayload;
}

int8_t bmac_rx_pkt_ready (void)
{
  return (!rx_buf_empty);
}

int8_t bmac_rx_pkt_release (void)
{
  rx_buf_empty = 1;
  return NRK_OK;
}

void bmac_disable ()
{
  is_enabled = 0;
  rf_power_down ();
}

void bmac_enable ()
{
  is_enabled = 1;
  rf_power_up ();
  nrk_event_signal (bmac_enable_signal);
}


void bmac_nw_task ()
{
  int8_t v, i;
  int8_t e;
  uint8_t backoff;
  nrk_sig_mask_t event;

  while (bmac_started () == 0)
    nrk_wait_until_next_period ();

//register the signal after bmac_init has been called
  v = nrk_signal_register (bmac_enable_signal);
  if (v == NRK_ERROR)
    nrk_kprintf (PSTR ("Failed to register signal\r\n"));
  backoff = 0;
  while (1) {
#ifdef NRK_SW_WDT
#ifdef BMAC_SW_WDT_ID
    nrk_sw_wdt_update (BMAC_SW_WDT_ID);
#endif
#endif
    rf_power_up ();
    if (is_enabled) {
      v = 1;

#ifdef BMAC_MOD_CCA
      if (rx_buf_empty == 1)
      {
	 if (_bmac_rx () == 1) e = nrk_event_signal (bmac_rx_pkt_signal);
      }
      else
      e = nrk_event_signal (bmac_rx_pkt_signal);
#else
      if (rx_buf_empty == 1)
        v = _bmac_channel_check ();
      // If the buffer is full, signal the receiving task again.
      else
        e = nrk_event_signal (bmac_rx_pkt_signal);
      // bmac_channel check turns on radio, don't turn off if
      // data is coming.

      if (v == 0) {
        if (_bmac_rx () == 1) {
          e = nrk_event_signal (bmac_rx_pkt_signal);
          //if(e==NRK_ERROR) {
          //      nrk_kprintf( PSTR("bmac rx pkt signal failed\r\n"));
          //      printf( "errno: %u \r\n",nrk_errno_get() );
          //}
        }
        //else nrk_kprintf( PSTR("Pkt failed, buf could be corrupt\r\n" ));

      }

#endif
      if (tx_data_ready == 1) {
        _bmac_tx ();
      }
      rf_rx_off ();
      rf_power_down ();

      //do {
      nrk_wait (_bmac_check_period);
      //      if(rx_buf_empty!=1)  nrk_event_signal (bmac_rx_pkt_signal);
      //} while(rx_buf_empty!=1);
    }
    else {
      event = 0;
      do {
        v = nrk_signal_register (bmac_enable_signal);
        event = nrk_event_wait (SIG (bmac_enable_signal));
      }
      while ((event & SIG (bmac_enable_signal)) == 0);
    }
    //nrk_wait_until_next_period();
  }

}


int8_t bmac_set_rx_check_rate (nrk_time_t period)
{
  if (period.secs == 0
      && period.nano_secs < BMAC_MIN_CHECK_RATE_MS * NANOS_PER_MS)
    return NRK_ERROR;
  _bmac_check_period.secs = period.secs;
  _bmac_check_period.nano_secs = period.nano_secs;
  return NRK_OK;
}

int8_t bmac_started ()
{
  return bmac_running;
}

int8_t _bmac_channel_check ()
{
  int8_t val = 0;

  rf_rx_on ();
  val += rf_cca_check ();
  val += rf_cca_check ();
  val += rf_cca_check ();
  if (val > 1)
    val = 1;
  rf_rx_off ();
  return val;
}

// Assuming that CCA returned 1 and a packet is on its way
// Receive the packet or timeout and error
int8_t _bmac_rx ()
{
  int8_t n;
  uint8_t cnt;

  rf_rx_on ();
  cnt = 0;
//printf( "calling rx\r\n" );
  dummy_t.secs = 0;
  dummy_t.nano_secs = 5 * NANOS_PER_MS;
  nrk_wait (dummy_t);

  n = rf_rx_packet_nonblock ();

  if (n != NRK_OK) {
    if (rx_failure_cnt < 65535)
      rx_failure_cnt++;
    rf_rx_off ();
    return 0;
  }

/*while ( rf_rx_packet_nonblock() != NRK_OK )
	{
	cnt++;
	nrk_wait(_bmac_check_period);
	if(cnt>2) { 
			#ifdef DEBUG
			printf( "rx timeout 1 %d\r\n",cnt );
			#endif
			if(rx_failure_cnt<65535) rx_failure_cnt++;
			rf_rx_off();
			return 0;
			} 
	}
*/


  rx_buf_empty = 0;
#ifdef DEBUG
  printf ("BMAC: SNR= %d [", bmac_rfRxInfo.rssi);
  for (uint8_t i = 0; i < bmac_rfRxInfo.length; i++)
    printf ("%c", bmac_rfRxInfo.pPayload[i]);
  printf ("]\r\n");
#endif
  rf_rx_off ();
  return 1;
}


uint16_t bmac_rx_failure_count_get ()
{
  return rx_failure_cnt;
}

uint8_t bmac_rx_failure_count_reset ()
{
  rx_failure_cnt = 0;
  return NRK_OK;
}

int8_t _bmac_tx ()
{
  uint8_t v, backoff, backoff_count;
  uint16_t b;

#ifdef DEBUG
  nrk_kprintf (PSTR ("_bmac_tx()\r\n"));
#endif
  if (cca_active) {

// Add random time here to stop nodes from synchronizing with eachother
    b = _nrk_time_to_ticks (&_bmac_check_period);
    b = b / ((rand () % 10) + 1);
//printf( "waiting %d\r\n",b );
    nrk_wait_until_ticks (b);
//nrk_wait_ticks(b);

    backoff_count = 1;
    do {
#ifdef BMAC_MOD_CCA
	v=_bmac_rx();
	v=!v;
        if (v == 1) { 
		break; 
	}
        nrk_event_signal (bmac_rx_pkt_signal);
#else
      v = _bmac_channel_check ();
      if (v == 1)
        break;
#endif
      // Channel is busy
      backoff = rand () % (_b_pow (backoff_count));
#ifdef DEBUG
      printf ("backoff %d\r\n", backoff);
#endif
//      printf( "backoff %d\r\n",backoff );
      nrk_wait_until_next_n_periods (backoff);
      backoff_count++;
      if (backoff_count > 6)
        backoff_count = 6;      // cap it at 64    
      b = _nrk_time_to_ticks (&_bmac_check_period);
      b = b / ((rand () % 10) + 1);
//      printf( "waiting %d\r\n",b );
      nrk_wait_until_ticks (b);
//      nrk_wait_ticks(b);

    }
    while (v == 0);
  }

  // send extended preamble
  bmac_rfTxInfo.cca = 0;
  bmac_rfTxInfo.ackRequest = 0;

  uint16_t ms = _bmac_check_period.secs * 1000;
  ms += _bmac_check_period.nano_secs / 1000000;
  //printf( "CR ms: %u\n",ms );
  //target_t.nano_secs+=20*NANOS_PER_MS;
  rf_rx_on ();
  pkt_got_ack = rf_tx_packet_repeat (&bmac_rfTxInfo, ms);

  // send packet
  // pkt_got_ack=rf_tx_packet (&bmac_rfTxInfo);
  rf_rx_off ();                 // Just in case auto-ack left radio on
  tx_data_ready = 0;
  nrk_event_signal (bmac_tx_pkt_done_signal);
  return NRK_OK;
}

uint8_t _b_pow (uint8_t in)
{
  uint8_t i;
  uint8_t result;
  if (in <= 1)
    return 1;
  if (in > 7)
    in = 6;                     // cap it at 128 
  result = 1;
  for (i = 0; i < in; i++)
    result = result * 2;
  return result;
}


void bmac_task_config ()
{
  nrk_task_set_entry_function (&bmac_task, bmac_nw_task);
  nrk_task_set_stk (&bmac_task, bmac_task_stack, BMAC_STACKSIZE);
  bmac_task.prio = BMAC_TASK_PRIORITY;
  bmac_task.FirstActivation = TRUE;
  bmac_task.Type = BASIC_TASK;
  bmac_task.SchType = PREEMPTIVE;
  bmac_task.period.secs = 0;
  bmac_task.period.nano_secs = BMAC_MIN_CHECK_RATE_MS * NANOS_PER_MS;
  bmac_task.cpu_reserve.secs = 0;       // bmac reserve , 0 to disable
  bmac_task.cpu_reserve.nano_secs = 0;
  bmac_task.offset.secs = 0;
  bmac_task.offset.nano_secs = 0;
#ifdef DEBUG
  printf ("bmac activate\r\n");
#endif
  nrk_activate_task (&bmac_task);
}
